#!/bin/bash

#请这样使用：
# chmod +x ros2_recorder.sh
# bash ros2_recorder.sh (my_bag)
# ros2 bag play my_bag

topics_must_to_record=""
topics_must_to_record+=" /apm_drone/target_mode"
topics_must_to_record+=" /apm_drone/target_cmd_vel"
topics_must_to_record+=" /apm_drone/high_permission_target_cmd_vel"
topics_must_to_record+=" /apm_drone/current_velocity"
topics_must_to_record+=" /apm_drone/current_mode_state"
topics_must_to_record+=" /apm_drone/current_mode"
topics_must_to_record+=" /apm_drone/current_attitude"
topics_must_to_record+=" /apm_drone/current_GPS"
topics_must_to_record+=" /apm_drone/current_battery"
topics_must_to_record+=" /apm_drone/current_local_location"
topics_must_to_record+=" /apm_drone/home_gps_location"
topics_must_to_record+=" /apm_drone/target_gps_location"
topics_must_to_record+=" /scan"
topics_must_to_record+=" /cmd_vel"
topics_must_to_record+=" /map"
topics_must_to_record+=" /local_costmap/published_footprint"
# topics_must_to_record+=" /global_costmap/costmap"
# topics_must_to_record+=" /global_costmap/costmap_updates"
topics_must_to_record+=" /plan"
topics_must_to_record+=" /local_costmap/costmap"
topics_must_to_record+=" /local_costmap/costmap_updates"
topics_must_to_record+=" /trajectories"
topics_must_to_record+=" /camera"
topics_must_to_record+=" /system_info"
topics_must_to_record+=" /tf"
topics_must_to_record+=" /tf_static"
topics_must_to_record+=" "
topics_must_to_record+=" "
topics_must_to_record+=" "
topics_must_to_record+=" "
topics_must_to_record+=" "

today=$(date +%Y-%m-%d)
current_time=$(date +%H.%M.%S)

if [ $# -eq 0 ]; then
    output_directory=~/ros2_recorder/$today/$current_time
    echo "No output directory specified. Using default directory: $output_directory"
else
    output_directory=$1
fi

exclude_topics="(/rosout /parameter_events /_hidden_topics /global_costmap/costmap /global_costmap/costmap_updates)"

found_topics=0
while [ $found_topics -eq 0 ]; do
    all_topics=$(ros2 topic list)
    
    topics_to_record=""
    for topic in $all_topics; do
        if [[ ! $exclude_topics =~ $topic ]]; then
            topics_to_record+=" $topic"
            found_topics=1
        fi
    done

    if [ $found_topics -eq 0 ]; then
        echo "No non-default topics found. Waiting..."
        sleep 1
    fi
done

echo "Recording topics: $topics_to_record"
# 添加分段参数：每60秒（60000毫秒）分割一次
ros2 bag record -o $output_directory --max-bag-duration 60000 $topics_to_record $topics_must_to_record
